import sensor, lcd
import time
from modules import ybkey
from modules import ybrgb
from fpioa_manager import fm
from machine import UART

RGB = ybrgb()
KEY = ybkey()
RGB.set(0, 1, 0)
# binding UART2 IO:6->RX, 8->TX
fm.register(6, fm.fpioa.UART2_RX)
fm.register(8, fm.fpioa.UART2_TX)
Uart = UART(UART.UART2, 9600, 8, 0, 0, timeout=1000, read_buf_len=4096)
lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)   # QVGA:320x240 - QQVGA:160x120

sensor.set_auto_exposure(1)                                 # 设置自动曝光
#sensor.set_auto_exposure(0, exposure=120000)               # 设置手动曝光 曝光时间 120000 us
sensor.set_auto_gain(0, gain_db = 17)                       # 设置画面增益 17 dB 影响实时画面亮度
sensor.set_auto_whitebal(0, rgb_gain_db = (0,0,0))          # 设置RGB增益 0 0 0 dB 影响画面色彩呈现效果 在 K210 上无法调节增益 初步判定是感光元件 ov2640 无法支持
sensor.run(1)
sensor.skip_frames(time = 2000)

clock = time.clock()
#*********************************************
red_thresholds = [(81, 100, 2, 127, -128, 127)]
green_thresholds = [(81, 100, -41, -15, -128, 127)]
roi_ = [56,35,209,143]
rect_ = 0
red_point_ = [0,0]
green_point_ = [0,0]
#*********************************************

# 计算两点距离
def point_distance(a,b):
    return (abs(a[0]-b[0])**2 + abs(a[1]-b[1])**2)**0.5

# 找矩形
def find_rect(img):
    global rect_
    magnitude = 0
    for r in img.find_rects(roi=roi_,threshold=50000):
        # 查找最大面积值
        if r.magnitude() > magnitude:
            rect_ = r

# 找光点:根据颜色全局搜索，找不到在从方框中逐个像素点查找
def _find_point_(img,threshold_,r_g):
    global rect_
    point_ = [0,0]
    area = 0
    for r in img.find_blobs(threshold_,roi=roi_):
        if r.area() > area:
            area = r.area()
            # point_ = r
            point_ = [r.cx(),r.cy()]
    # 没有找到光点在继续从方框中逐个像素点扫描（用于扫描被黑线吃掉的光点）
    if area == 0 and rect_ != 0:
        for x in range(rect_.x(),rect_.x()+rect_.w()):
            for y in range(rect_.y(),rect_.y()+rect_.h()):
                if r_g == 0 :
                    rgb = img.get_pixel(x,y)[0]
                else :
                    rgb = img.get_pixel(x,y)[1]
                if rgb > 250 :
                    if (r_g == 0 and point_distance([x,y],green_point_) > 8) or (r_g == 1 and point_distance([x,y],red_point_) > 8):
                        point_[0] = x
                        point_[1] = y
                        return point_
    return point_

def find_point(img):
    global red_point_,green_point_
    red_point_ = _find_point_(img,red_thresholds,0)
    green_point_ = _find_point_(img,green_thresholds,1)

# 画矩形画圆点
def draw(img):
    # 画矩形
    if rect_ != 0:
        img.draw_rectangle(rect_.rect(), color = (255, 0, 0), thickness=1)
        for p in rect_.corners():
            img.draw_string (p[0], p[1], str(p[0])+','+str(p[1]), color=(255, 255, 255),mono_space=False)
    # 画圆点
    img.draw_rectangle(tuple(roi_), color = (255, 0, 0), thickness=1)
    if red_point_[0] != 0:
        img.draw_circle(red_point_[0], red_point_[1], 10, thickness=2, color = (255, 0, 0))
        img.draw_string (red_point_[0], red_point_[1]+10, str(red_point_[0])+','+str(red_point_[1]), color=(255, 255, 255),mono_space=False)
    img.draw_rectangle(tuple(roi_), color = (255, 0, 0), thickness=1)
    if green_point_[0] != 0:
        img.draw_circle(green_point_[0], green_point_[1], 10, thickness=2, color = (0, 255, 0))
        img.draw_string (green_point_[0], green_point_[1]+10, str(green_point_[0])+','+str(green_point_[1]), color=(255, 255, 255),mono_space=False)

# 按键检测更新ROI区域
def led_flashing(color,count):
    for c in range(0,count) :
        if color == 'r':
            RGB.set(1, 0, 0)
        else :
            RGB.set(0, 0, 1)
        time.sleep(0.1)
        RGB.set(0, 1, 0)
        time.sleep(0.1)
def key_click():
    global rect_
    if KEY.is_press() == 1:
        size = 20
        if rect_ != 0:
            led_flashing('r',3)
            roi_[0] = rect_.x() - size
            roi_[1] = rect_.y() - size
            roi_[2] = rect_.w() + size * 2
            roi_[3] = rect_.h() + size * 2
        else :  # 如果没有找到矩形就将ROI缩小
            led_flashing('b',3)
            roi_[0] = roi_[0] + 2
            roi_[1] = roi_[1] + 2
            roi_[2] = roi_[2] - 4
            roi_[3] = roi_[3] - 4

def Usart_send(img):
    if red_point_[0] != 0 and green_point_[0] != 0:
        data = [0xa0,0xb0]
        data.append(red_point_[0])
        data.append(red_point_[1])
        data.append(green_point_[0])
        data.append(green_point_[1])
        data.append(0xc0)
        print(data)
        img.draw_string(0, 5, "%d x" %(red_point_[0]), color=(0, 0, 255), scale=1.0)
        Uart.write(bytearray(data))

while(True):
    clock.tick()
    img = sensor.snapshot()
    fps = clock.fps()

    key_click()         # 按键检测
    find_rect(img)      # 矩形检测
    find_point(img)     # 光点检测
    Usart_send(img)        # 发送数据
    draw(img)           # 画框画点

    img.draw_string(0, 0, "%2.1ffps" %(fps), color=(0, 0, 255), scale=1.0)
    lcd.display(img)
    print(fps)


